/*
 * GoStraightAction.cpp
 *
 *  Created on: 13/mag/2014
 *      Author: va2013
 */

#include "GoStraightAction.h"

GoStraightAction::GoStraightAction(ArRobot* r, int maxVelocity, bool* mustCurve,
		uint16_t distanceToCurve, bool* emergencyStop) :
		ArAction("Advance", "Effettua l'avvicinamento ad un setpoint") {
	this->maxVel = maxVelocity;
	this->mustCurve = mustCurve;
	this->distanceToCurve = distanceToCurve;
	robot = r;
	this->completedDistance = false;
	this->emergencyStop = emergencyStop;
}

GoStraightAction::~GoStraightAction() {
	// TODO Auto-generated destructor stub
}

ArActionDesired* GoStraightAction::fire(ArActionDesired currentDesired) {
//	std::cout << "\GoStraightAction...\n";

	myDesired.reset();
	double vel = robot->getVel();
	if(vel<500)
	myDesired.setTransDecel(600);
	else
		myDesired.setTransDecel(1200);

	myDesired.setTransAccel(300);

	if (!(*emergencyStop)) {

		myDesired.setVel(maxVel, 0.50);
		double currentOdometer = robot->getOdometerDistance();
		double odometerDistance = currentOdometer - startingOdometer;
		if (odometerDistance >= distance) {
			myDesired.setVel(0);
			completedDistance = true;

			//std::cout<< "oD > distance"<<std::endl;

		} else if (odometerDistance > (double) (distance - distanceToCurve)) {
			*mustCurve = true;

			//std::cout<< "curve"<<std::endl;
		}
	}
	else//stop di emergenza
	{
		myDesired.setVel(0);
		*mustCurve = true;
		completedDistance = true;
	}
	return &myDesired;
}

bool GoStraightAction::haveAchievedDistance() {
	return completedDistance;
}

void GoStraightAction::setDistance(uint16_t distanceInMm) {
	this->distance = distanceInMm;
	this->startingOdometer = robot->getOdometerDistance();
	this->completedDistance = false;
	*mustCurve = false;

}
